import numpy as np

def load_lidar_data(path):
    """
    从文件中逐行读取并解析 LiDAR 数据。
    """
    # 初始化一个列表来存储所有帧数据
    data_list = []

    # 初始化一个临时字典来存储当前帧数据
    current_frame = {}
    points = []  # 用于存储当前帧的点云数据

    try:
        with open(path, 'r', encoding='utf-8', errors='ignore') as file:
            for line in file:
                line = line.strip()  # 去掉空格和换行符

                if line.startswith('time_stamp:'):
                    # 如果当前帧有数据，将其存储到 data_list
                    if current_frame:
                        current_frame['points'] = np.array(points)  # 将点云数据转换为 NumPy 数组
                        current_frame['pcl_no'] = len(points)  # 计算点云数量
                        data_list.append(current_frame)
                        current_frame = {}
                        points = []

                    # 解析时间戳
                    current_frame['time_stamp'] = int(line.split(': ')[1])

                elif line.startswith('fusepose:'):
                    # 解析 fusepose
                    current_frame['fusepose'] = list(map(float, line.split(': ')[1].split()))

                elif line.startswith('rpy:'):
                    # 解析 rpy
                    current_frame['rpy'] = list(map(float, line.split(': ')[1].split()))

                else:
                    # 解析点云数据
                    if line:  # 确保不是空行
                        points.append(list(map(float, line.split())))

        # 保存最后一帧数据
        if current_frame:
            current_frame['points'] = np.array(points)
            current_frame['pcl_no'] = len(points)
            data_list.append(current_frame)

    except Exception as e:
        print(f"读取文件失败: {e}")

    return data_list


def load_ai_data(path):
    """
    从文件中逐行读取并解析 AI 数据。
    """
    frames = []
    current_frame = {}

    try:
        with open(path, 'r', encoding='utf-8', errors='ignore') as file:
            for line in file:
                line = line.strip()

                if line.startswith('time_stamp:'):
                    # 如果当前帧有数据，将其存储到 frames
                    if current_frame:
                        frames.append(current_frame)
                        current_frame = {}

                    # 创建新的帧并存储时间戳
                    current_frame = {'time_stamp': int(line.split(': ')[1]), 'xyz': []}

                elif line.startswith('xyz:'):
                    # 存储 xyz 数据，避免重复
                    xyz_value = list(map(float, line.split(': ')[1].split()))
                    if xyz_value not in current_frame['xyz']:
                        current_frame['xyz'].append(xyz_value)

        # 保存最后一帧数据
        if current_frame:
            frames.append(current_frame)

    except Exception as e:
        print(f"读取文件失败: {e}")

    return frames


def load_lidar_ai_data(li_path, ai_path):
    """
    加载并解析 LiDAR 和 AI 数据。
    """
    ai_data = load_ai_data(ai_path)
    li_data = load_lidar_data(li_path)
    return ai_data, li_data


def matching_pose_with_ai(ai_data, li_data):
    """
    匹配 LiDAR 数据和 AI 数据，基于时间戳差异进行匹配。
    """
    result = []
    i, j = 0, 0
    threshold = 50  # 时间戳匹配阈值
    index = 0
    while i < len(li_data) and j < len(ai_data):
        # 计算时间戳差异
        diff = li_data[i]["time_stamp"] - ai_data[j]["time_stamp"]

        if abs(diff) < threshold:
            # 匹配成功，合并数据
            tmp = li_data[i].copy()
            tmp['xyz'] = ai_data[j]['xyz']
            if tmp['xyz'].__len__() != 0:
                print(index, ai_data[j]["time_stamp"])
            tmp['time_stamp1'] = ai_data[j]['time_stamp']
            pcl_no_len = tmp['xyz'].__len__()
            tmp['diff'] = diff
            tmp['pcl_no'] = pcl_no_len
            tmp['points'] = tmp['xyz']
            result.append(tmp)
            index += 1
            j += 1  # 移动 AI 数据指针
        elif diff < 0:
            i += 1  # LiDAR 数据时间戳较小，移动 LiDAR 数据指针
        else:
            j += 1  # AI 数据时间戳较小，移动 AI 数据指针

    return result


if __name__ == '__main__':
    # LiDAR 和 AI 数据文件路径
    li_path = '/home/JSDC/017254/code/gitee/map_learing/03_map_grid_python/04_xyzrpy_visual/rpy/data4/li.txt'
    ai_path = '/home/JSDC/017254/code/gitee/map_learing/03_map_grid_python/04_xyzrpy_visual/rpy/data4/ai.txt'

    # 加载数据
    ai_data, li_data = load_lidar_ai_data(li_path, ai_path)

    # 匹配数据
    res = matching_pose_with_ai(ai_data, li_data)

    # 打印结果
    for frame in res:
        print(frame)


